blog
开普勒根数与位置速度互相转化
地球卫星工作中，经常要从开普勒根数与位置速度相互转化。下面给出代码。
program main
!--------------------------------------program comment
! Version : V1.0
! Author : song.yz
! Created : 2018-03-15 18:07:35
! Changes :
! *.
!-----------------------------------------------------
! Notes :
! *.
!
!-----------------------------------------------------
implicit real*8(a-h,o-z)
real*8 i, m, pos(3),vel(3)
pi= 3.141592653589793238462643d0
emu = 3.98600441500000D+14
open(unit=11,file='sat.ele')
open(unit=12,file='sat.ics')
open(unit=13,file='satx.ele')
do
read(11,*,iostat=ioend)isat,a,ec,i,capom,omeg,m
if (ioend /= 0) exit
i= i*pi/180d0
capom =capom*pi/180d0
omeg =omeg*pi/180d0
!capom =capom*pi/180d0
m = m*pi/180
!call kepcar(a, ec, i, omeg, capom, m, emu, r, v)
call orb2xyz(a,ec,i,capom,omeg,M,pos,vel)
call xyz2orb(pos,vel,a,ec,i,capom,omeg,M)
write(12,'(I5,6F22.8)') isat,pos,vel
write(13,'(I5,6F22.8)') isat, a,ec,i*180/pi,capom*180/pi,omeg*180/pi,M*180/pi
end do
close(11)
close(12)
end program main
subroutine orb2xyz(a,ecc,inc,node,perigee,M,pos,vel)
!-----------------------------------------------------
! Purpose : 2022-5-7 12:46
! kepler根数转位置速度
!-----------------------------------------------------
! Input Parameters :
! a ---单位：米
! inc ,node,perigee,M ---- 单位：弧度
! ecc
! Output Parameters :
! pos(3) ----- 单位 ：米
! vel(3) ---- 单位 ： 米/秒
!-----------------------------------------------------
! Author : Song Yezhi
! Copyrigt (C) : Chinese Academy of Sciences
! All rights reserved, 2022
!-----------------------------------------------------
implicit none
real*8 ,Parameter :: GM = 3.9860044d14
real*8 ::a,ecc,inc,node,perigee,M,pos(3),vel(3)
real*8 :: P(3),Q(3)
real*8 :: E0,E ,r
integer :: k
P(1) = dcos(perigee)*dcos(node) - dsin(perigee)*dcos(inc)* dsin(node)
P(2) = dcos(perigee)*dsin(node) + dsin(perigee)*dcos(inc)* dcos(node)
P(3) = dsin(perigee)*dsin(inc)
Q(1) = -dsin(perigee)*dcos(node) - dcos(perigee) * dcos(inc) *dsin(node)
Q(2) = -dsin(perigee)*dsin(node) + dcos(perigee) * dcos(inc) *dcos(node)
Q(3) = dcos(perigee) *dsin(inc)
E0 = M
do k = 1, 30
E = E0 - ( E0-ecc*dsin(E0) -M)/(1d0 - ecc*dcos(E0 ))
if (dabs(E0-E ) <1d-9) exit
E0 = E
end do
pos = a*(dcos(E)-ecc) * P + a*dsqrt(1d0-ecc*ecc) *dsin(E) *Q
r = dsqrt(pos(1)*pos(1)+pos(2)*pos(2)+pos(3)*pos(3))
vel = dsqrt(GM * a) / r *(-dsin(E)*P + dsqrt(1d0 -ecc*ecc)*dcos(E)*Q)
end subroutine orb2xyz
subroutine xyz2orb(pos,vel,a,ecc,inc,node,perigee,M)
!-----------------------------------------------------
! Purpose : 2022-5-7 13:31
! from position and velocity to kepler elements
!-----------------------------------------------------
! Input Parameters :
! pos(3)
! vel(3) 单位 米/秒
! Output Parameters :
! a,inc,node,perigee,M ---单位：弧度
! ecc
!-----------------------------------------------------
! Author : Song Yezhi
! Copyrigt (C) : Chinese Academy of Sciences
! All rights reserved, 2022
!-----------------------------------------------------
implicit none
real*8 ,Parameter :: GM = 3.9860044d14
real*8 ::a,ecc,inc,node,perigee,M,pos(3),vel(3)
real*8 :: h(3) ,r,h2,p ,n, W(3),v2
real*8 :: u,miu ,E
h(1) = pos(2)*vel(3) - pos(3)*vel(2)
h(2) = pos(3)*vel(1) - pos(1)*vel(3)
h(3) = pos(1)*vel(2) - pos(2)*vel(1)
h2 = h(1)*h(1) + h(2)*h(2) +h(3)*h(3)
W = h/dsqrt(h2)
inc = datan(dsqrt(h(1)*h(1)+h(2)*h(2))/h(3))
node = datan2(W(1),-W(2))
r = dsqrt(pos(1)*pos(1)+pos(2)*pos(2)+pos(3)*pos(3))
v2 = vel(1)*vel(1) + vel(2)*vel(2) + vel(3)*vel(3)
a = 1d0/ (2d0/r - v2/GM )
p = h2 /GM
ecc = dsqrt(1d0 - p/a)
n = dsqrt(GM/a**3)
E =datan2( (pos(1)*vel(1)+pos(2)*vel(2)+pos(3)*vel(3))/(a*a*n), 1d0 - r/a )
M = E - ecc *dsin(E)
u = datan2(pos(3),-pos(1)*W(2)+pos(2)*W(1))
miu = datan2(dsqrt(1d0-ecc*ecc)*dsin(E),dcos(E)-ecc)
perigee = u -miu
!--------nominate the angles to 0-2pi
call ANP(inc,inc)
call ANP(node,node)
call ANP(perigee,perigee)
call ANP(M,M)
end subroutine xyz2orb
subroutine ANP(A,nominalA)
!-----------------------------------------------------
! Purpose : 2022-6-7 14:21
! Normalize angle into the range 0 <= A < 2pi.
!
! Given:
! A d angle (radians)
!
! Returned:
! ANP d angle in range 0-2pi
!-----------------------------------------------------
! Author : Song Yezhi
! Copyrigt (C) : Chinese Academy of Sciences
! All rights reserved, 2022
!-----------------------------------------------------
implicit none
real*8 :: A,nominalA
real*8,parameter :: D2PI = 6.283185307179586476925287D0
nominalA = MOD(A,D2PI)
IF ( nominalA .LT. 0D0 ) nominalA = nominalA + D2PI
end subroutine ANP
1 27878137.00000000 0.01000000 55.00000000 0.00000000 0.00000000 0.00000000
2 27878137.00000000 0.01000000 55.00000000 0.00000000 0.00000000 45.00000000
3 27878137.00000000 0.01000000 55.00000000 0.00000000 0.00000000 90.00000000
4 27878137.00000000 0.01000000 55.00000000 0.00000000 0.00000000 135.00000000
5 27878137.00000000 0.01000000 55.00000000 0.00000000 0.00000000 180.00000000
6 27878137.00000000 0.01000000 55.00000000 0.00000000 0.00000000 225.00000000
7 27878137.00000000 0.01000000 55.00000000 0.00000000 0.00000000 270.00000000
输出结果为
1 27599355.63000000 0.00000000 0.00000000 0.00000000 2190.64197349 3128.56096793
2 19293159.92175293 11386470.67590320 16261565.39961870 -2711.76808091 1533.33301912 2189.82649493
3 -557544.15680526 15988643.55490426 22834149.42344667 -3780.69703245 -21.68374166 -30.96759243
4 -20129522.61494722 11226581.57732551 16033220.10218516 -2636.14784164 -1533.33880196 -2189.83475367
5 -28156918.37000000 0.00000000 0.00000000 0.00000000 -2147.26292451 -3066.60926559
6 -20129522.61494722 -11226581.57732551 -16033220.10218516 2636.14784164 -1533.33880196 -2189.83475367
7 -557544.15680527 -15988643.55490426 -22834149.42344667 3780.69703245 -21.68374166 -30.96759243